/*
 * ARDroneCommand.cpp
 *
 *  Created on: 16.07.2012
 *      Author: seb
 */

#include "DroneCommand.h"

DroneCommand::DroneCommand(float roll, float pitch, float yaw, float vertical, int specialCommand) {
	this->roll = roll;
	this->pitch = pitch;
	this->yawSpeed = yaw;
	this->vSpeed = vertical;
	this->specialCommand = specialCommand;
}

DroneCommand::DroneCommand(const DroneCommand& dc) {
	this->roll = dc.getRoll();
	this->pitch = dc.getPitch();
	this->yawSpeed = dc.getYaw();
	this->vSpeed = dc.getVertical();
	this->specialCommand = dc.getSpecialCommand();
}

/*
 * Create Command from String
 */
DroneCommand::DroneCommand(string command) {
	this->fromString(command);
}

float DroneCommand::getPitch() const {
	return pitch;
}

void DroneCommand::setPitch(float pitch) {
	this->pitch = checkMinMaxOfValue(pitch);
}

float DroneCommand::getRoll() const {
	return roll;
}

void DroneCommand::setRoll(float roll) {
	this->roll = checkMinMaxOfValue(roll);
}

float DroneCommand::getVertical() const {
	return vSpeed;
}

void DroneCommand::setVertical(float speed) {
	this->vSpeed = checkMinMaxOfValue(speed);
}

float DroneCommand::getYaw() const {
	return yawSpeed;
}

void DroneCommand::setYaw(float yawSpeed) {
	this->yawSpeed = checkMinMaxOfValue(yawSpeed);
}

int DroneCommand::getSpecialCommand() const {
	return this->specialCommand;
}

void DroneCommand::setSpecialCommand(int specialCommand) {
	this->specialCommand = specialCommand;
}

float DroneCommand::checkMinMaxOfValue(float val) {
	return fmaxf(-1.0, fminf(1.0, val));
}

string DroneCommand::floatToString(float val) {
	return boost::lexical_cast<string>( val);
}

float DroneCommand::stringToFloat(string val) {
	return boost::lexical_cast<float>(val);
}



void DroneCommand::printToConsole() {
	switch(this->specialCommand) {
	case DRONE_SPECIAL_COMMAND_NONE:
		printf("MOVE:  r: %4.3f p: %4.3f y: %4.3f v: %4.3f \n", this->getRoll(), this->getPitch(), this->getYaw(), this->getVertical() );
		break;
	case DRONE_SPECIAL_COMMAND_START:
		printf("START \n");
		break;
	case DRONE_SPECIAL_COMMAND_LAND:
		printf("LAND \n");
		break;
	case DRONE_SPECIAL_COMMAND_HOVER:
		printf("HOVER: r: %4.3f p: %4.3f y: %4.3f v: %4.3f \n", this->getRoll(), this->getPitch(), this->getYaw(), this->getVertical() );
		break;
	case DRONE_SPECIAL_COMMAND_EMERGENCY:
		printf("EMERGENCY \n");
		break;
	}

}

/*************************************************************************************************************************
* 4 char * 4 char  * 1 float * 1 float * 1 float * 1 float *
* Header * special * roll	 * pitch   * vertical* yaw     *
*		 * command *		 *		   *		 *		   *
**************************************************************************************************************************/
// TODO optimizing code, to not send a whole float, values should be in range -1.000 - 1.000

string DroneCommand::toString() {
	int buffSize = DRONE_COMMAND_HEADER_LENGTH + DRONE_NET_LENGTH + 4 * sizeof(float);
	char buff[buffSize];
	int buffSaturation = 0;
	buffSaturation = sprintf(buff + buffSaturation, DRONE_COMMAND_HEADER );

	//TODO
	switch (this->specialCommand) {
		case DRONE_SPECIAL_COMMAND_NONE:
			buffSaturation = sprintf(buff + buffSaturation, DRONE_NET_NONE );
			break;
		case DRONE_SPECIAL_COMMAND_START:
			buffSaturation = sprintf(buff + buffSaturation, DRONE_NET_START );
			break;
		case DRONE_SPECIAL_COMMAND_LAND:
			buffSaturation = sprintf(buff + buffSaturation, DRONE_NET_LAND );
			break;
		case DRONE_SPECIAL_COMMAND_EMERGENCY:
			buffSaturation = sprintf(buff + buffSaturation, DRONE_NET_EMERGENCY );
			break;
		default:
			//assume case DRONE_SPECIAL_COMMAND_HOVER
			buffSaturation = sprintf(buff + buffSaturation, DRONE_NET_HOVER );
		break;
	}
	if (this->specialCommand == DRONE_SPECIAL_COMMAND_NONE) {
		buffSaturation = sprintf(buff + buffSaturation, "%f%f%f%f", this->roll, this->pitch, this->vSpeed, this->yawSpeed );
	} else {
		buffSaturation = sprintf(buff + buffSaturation, "%f%f%f%f", 0.0, 0.0, 0.0, 0.0 );
	}

	return (string) buff;
}

void DroneCommand::fromString(string command) {
	// TODO case not conform input
	int floatSize = sizeof(float);
	uint32_t commandLength = DRONE_COMMAND_HEADER_LENGTH + DRONE_NET_LENGTH + 4 * sizeof(float);
	if ( command.length() == commandLength) {
		if (command.substr(0, DRONE_COMMAND_HEADER_LENGTH ) == DRONE_COMMAND_HEADER ) {
			string specCom = command.substr(DRONE_COMMAND_HEADER_LENGTH, DRONE_NET_LENGTH );
			int offsetCommand = DRONE_COMMAND_HEADER_LENGTH + DRONE_NET_LENGTH;

			this->roll = 0.0;
			this->pitch = 0.0;
			this->vSpeed = 0.0;
			this->yawSpeed = 0.0;

			if (specCom == DRONE_NET_NONE) {
				this->specialCommand = DRONE_SPECIAL_COMMAND_NONE;

				this->roll = stringToFloat(command.substr(offsetCommand, floatSize));
				offsetCommand += floatSize;

				this->pitch = stringToFloat(command.substr(offsetCommand, floatSize));
				offsetCommand += floatSize;

				this->vSpeed = stringToFloat(command.substr(offsetCommand, floatSize));
				offsetCommand += floatSize;

				this->yawSpeed = stringToFloat(command.substr(offsetCommand, floatSize));
				offsetCommand += floatSize;

			}else if ( specCom == DRONE_NET_START ) {
				this->specialCommand = DRONE_SPECIAL_COMMAND_START;

			} else if (specCom == DRONE_NET_LAND ) {
				this->specialCommand = DRONE_SPECIAL_COMMAND_LAND;

			} else if (specCom == DRONE_NET_EMERGENCY) {
				this->specialCommand = DRONE_SPECIAL_COMMAND_EMERGENCY;
			} else {
				this->specialCommand = DRONE_SPECIAL_COMMAND_HOVER;
			}
		}
	}

}

DroneCommand::~DroneCommand() {
	// TODO Auto-generated destructor stub
}

